#include "attachobjectmanipulator.h"
#include <QString>
#include <osg/math>
#include "pickmanager.h"
//#include "terrainmanager.h"

namespace ControlCenter
{
	AttachObjectManipulator::AttachObjectManipulator()
	{
		//_eyeVec = osg::Vec3(5470,0,_cameraHeight);
		//_centerVec = osg::Vec3(0,0,0);
		//_upVec = osg::Vec3(0,0,1);
		useCollisionCamera(false);
	}

	AttachObjectManipulator::~AttachObjectManipulator()
	{}

	void AttachObjectManipulator::objectTransformInitThis(const osg::Vec3& eyepos,const osg::Vec3& pos,const osg::Vec3& center,const osg::Vec3& rota,const osg::Vec3& up)
	{
		_homeEye = eyepos + pos;
		_homeUp = up;
		_homeCenter = center + pos;
		_baseEye = eyepos;
		_direction = _homeCenter - _homeEye;
		_direction.normalize();
		_directionBase = _direction;
		_position = _homeEye;
		_baseUp = _homeUp;
	}

	void AttachObjectManipulator::setObjectTransform(const osg::Vec3& pos,const osg::Vec3& rota)
	{
		if( fabs(_directionRotationRate + _rightMouseRotationRate) > _directionRotationEpsilon)
		{
			_directionBase = _directionBase * osg::Matrix::rotate(_directionRotationRate + _rightMouseRotationRate, osg::Vec3(0,0,1));
			_baseUp = _baseUp *osg::Matrix::rotate(_directionRotationRate + _rightMouseRotationRate, osg::Vec3(0,0,1));
		}
		osg::Vec3d _sideVec = _directionBase * osg::Matrix::rotate( -M_PI*0.5, _baseUp);
		_directionBase = _directionBase *osg::Matrix::rotate(_upDisctence, _sideVec);
		_baseUp = _baseUp *osg::Matrix::rotate(_upDisctence, _sideVec);

		_direction = _directionBase * osg::Matrix::rotate(rota.z()*osg::PI/180.0f, osg::Vec3(0,0,1));
		_homeUp =  _baseUp * osg::Matrix::rotate(rota.z()*osg::PI/180.0f, osg::Vec3(0,0,1));
		_position = _baseEye*osg::Matrix::rotate(rota.z()*osg::PI/180.0f, osg::Vec3(0,0,1))+pos;

		_inverseMatrix.makeLookAt( _position, _position + _direction, _homeUp);

		_rightMouseRotationRate = 0.0f;
		//_directionRotationRate = 0.0f;
		_upDisctence = 0.0f;
		_collisionOffsetVec3 = osg::Vec3(0,0,0);
	}

	void AttachObjectManipulator::home(double)
	{
		_position = _homeEye;
		_direction = _homeCenter - _homeEye;
		_direction.normalize();
		_directionBase = _direction;
		_directionRotationRate = 0.0;
		_rightMouseRotationRate = 0.0f;
		_inverseMatrix.makeLookAt( _homeEye, _homeCenter, _homeUp );
		_forwardSpeed = 0.0;
		_sideSpeed = 0.0;
		_upSpeed = 0.0;
	}

	void AttachObjectManipulator::_frame()
	{
		if(_lostFocus)
		{
			_rightMouseRotationRate = 0.0f;
			_directionRotationRate = 0.0;
			_forwardSpeed = 0.0;
			_sideSpeed = 0.0;
			_upSpeed = 0.0;
			_upDisctence = 0.0f;
			/*_collisionForwardSpeed = 0.0;
			_collisionSideSpeed = 0.0;*/
			_collisionOffsetVec3 = osg::Vec3(0,0,0);
			return;
		}
	}

	osg::Matrixd AttachObjectManipulator::getInverseMatrix() const
	{
		return _inverseMatrix;
	}

	void AttachObjectManipulator::enableManipulator()
	{}

	void AttachObjectManipulator::disenableManipulator()
	{}

	void AttachObjectManipulator::keyUpInterface(KeyInterfaceType type)
	{
		switch (type)
		{
		case Forward:
			_upDisctence = _upDisctence - 0.08f<- 0.08f-0.001f?- 0.08f:_upDisctence - 0.08f;
			break;
		case Backward:
			_upDisctence = _upDisctence + 0.080>0.080+0.001f?0.080:_upDisctence + 0.080;
			break;
		case Leftward:
			_directionRotationRate = _directionRotationRate - 0.08f<- 0.08f-0.001f?- 0.08f:_directionRotationRate - 0.08f;
			break;
		case Rightward:
			_directionRotationRate = _directionRotationRate + 0.080>0.080+0.001f?0.080:_directionRotationRate + 0.080;
			break;
		case Nearward:
		case Farward:
		case RotationL:
		case RotationR:
		case RotationU:
		case RotationD:
		default:
			break;
		}
	}

	void AttachObjectManipulator::keyDownInterface(KeyInterfaceType type)
	{
		switch (type)
		{
		case Forward:
			_upDisctence = _upDisctence + 0.08f>0.08f+0.001f?0.08f:_upDisctence + 0.080;
			break;
		case Backward:
			_upDisctence = _upDisctence - 0.08f<- 0.08f-0.001f?- 0.08f:_upDisctence - 0.08f;
			break;
		case Leftward:
			_directionRotationRate = _directionRotationRate + 0.08f>0.08f+0.001f?0.08f:_directionRotationRate + 0.080;
			break;
		case Rightward:
			_directionRotationRate = _directionRotationRate - 0.08f<- 0.08f-0.001f?- 0.08f:_directionRotationRate - 0.08f;
			break;
		case Nearward:
		case Farward:
		case RotationL:
		case RotationR:
		case RotationU:
		case RotationD:
		default:
			break;
		}
	}
}